/*
 * IC_PwmConfig.c
 *
 *  Created on: Nov 16, 2013
 *      Author: Administrador
 */
#include "Mytypes.h"
#include "IC_Headers/IC_PwmConfig.h"


#if FreescaleCup

#else
/*Init Fucntion*/
void vdfn_IC_PwmInit(){
	// clock source select PLL or FLL
	SIM_SOPT2 |= SIM_SOPT2_PLLFLLSEL_MASK; //Select PLL clock
	SIM_SOPT2 |= SIM_SOPT2_TPMSRC(1);	//MC6PLLCLK/2 = 48Mhz/2 = 24Mhz
	
	// System Clock Gating (Enable)
	SIM_SCGC6 |= SIM_SCGC6_TPM0_MASK;
	
	//Select the Counter max value 100% or 0xFFFF
	TPM0_MOD = PWM_MAX_COUNT;
		
	//Configures the PWM (No interrupt)
	TPM0_C0SC |=  TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK;	//For Center Aligned PWM
	TPM0_C1SC |=  TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK;	//Reverse
	TPM0_C2SC |=  TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK;
	TPM0_C3SC |=  TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK;	//Reverse
	

	//Configure the initial value for the counter in this case 50%
	TPM0_C0V = 0;
	TPM0_C1V = 0;
	TPM0_C2V = 0;
	TPM0_C3V = 0;

	
	//Enable the Counter
	TPM0_SC |= TPM_SC_CMOD(1);
		
}


void vdfn_IC_ServoInit(){
	// System Clock Gating (Enable)
	SIM_SCGC6 |= SIM_SCGC6_TPM1_MASK;
	
	//Preescaler
	TPM1_SC |= TPM_SC_PS(3);
	
	//Select the Counter max value 100% or 0xFFFF
	TPM1_MOD = PWM_20ms_PERIOD;	//50 Hz Frecuency
	
	//Configures the PWM SERVO (No interrupt)
	TPM1_C0SC |=  TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK;	//For Center Aligned PWM
	
	//Configure the initial value for the counter in this case 50%
	TPM1_C0V = 3300;
	
	//Enable the Counter
	TPM1_SC |= TPM_SC_CMOD(1);
	
}

void vdfn_IC_Joystick(uint8 ADC_Speed, uint8 ADC_Giro){ //Cambiar que recibimos 2 valores Giro y velocidad
	
	if(ADC_Speed > 164 && ADC_Speed < 209){
		vdfn_IC_Speed_Reverse(ADC_Speed);
	}
	
	else if(ADC_Speed > 73 && ADC_Speed < 118){
			vdfn_IC_Speed_Foward(ADC_Speed);
	}
	else{
		PWM_FOWARD_STOP_LEFT;		
		PWM_FOWARD_STOP_RIGHT;	
		PWM_REVERSE_STOP_LEFT;	
		PWM_REVERSE_STOP_RIGHT;		
	}
	
	if(ADC_Giro > 164 && ADC_Giro < 209){
		vdfn_IC_Turn_Left(ADC_Giro);
	}
	else if(ADC_Giro > 73 && ADC_Giro < 118){
		vdfn_IC_Turn_Right(ADC_Giro);
		}
	else{
		TPM1_C0V = 3300;
	}
	
		
}

void vdfn_IC_Speed_Foward(uint16 Speed){
	//Foward movement	
	H_BRIDGE_DIS;
	PWM_FOWARD_ON_LEFT;
	PWM_FOWARD_ON_RIGHT;
	Speed = (Speed * 266) - 11418;	//64min - 128max
	TPM0_C0V = Speed;
	TPM0_C2V = Speed;
	TPM0_C1V = 0;
	TPM0_C3V = 0;
	H_BRIDGE_EN;

	
}

void vdfn_IC_Speed_Reverse(uint16 Speed){
	
	//Reverse movement
	H_BRIDGE_DIS;
	PWM_REVERSE_ON_LEFT;
	PWM_REVERSE_ON_RIGHT;
	Speed = (Speed * 266) - 35624;	//191min - 255max
	TPM0_C1V = Speed;
	TPM0_C3V = Speed;
	TPM0_C0V = 0;
	TPM0_C2V = 0;
	H_BRIDGE_EN;
	
}

void vdfn_IC_Speed_Stop(){
	H_BRIDGE_DIS;
	PWM_FOWARD_ON_LEFT;
	PWM_FOWARD_ON_RIGHT;
	TPM0_C0V = 0;
	TPM0_C2V = 0;
	TPM0_C1V = 0;
	TPM0_C3V = 0;
	H_BRIDGE_EN;

}


void vdfn_IC_Turn_Right(uint16 Direction){
	TPM1_C0V =4500;	
}

void vdfn_IC_Turn_Left(uint16 Direction){
	TPM1_C0V =2500;
	
}

void vdfn_IC_Straight(){
	TPM1_C0V = 3300;
	
}

void vdfn_Pot_Active(){
	H_BRIDGE_DIS;
	Flag_Pot_Test = 1;
	H_BRIDGE_EN;
	
}

void vdfn_IC_Servo(){
	TPM1_C0V = (ADC_Test * 7) + 2400; //50Hz CLK
	//TPM1_C0V = (ADC_Test * 23) + 1500; //100Hz CLK
	
}

#endif


